/********** Library Includes **************************************************/

#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <sys/time.h>


/********** Local Includes ****************************************************/

#include "livesource.h"
#include "objectdetector.h"
#include "approxDecomp.h"
#include "common.h"
#include "celldecompastar.h"
#include "simpledrivecontroller.h"
#include "rovio.h"
#include "astarheuristics.h"

/********** Macros and Constants **********************************************/

#define DEBUG true
#define ROVIO_HOST "192.168.10.18"
#define TIME 500

/********** Namespaces ********************************************************/

using namespace cv;
using namespace std;

/********** Local Functions ***************************************************/

Point robot(231, 241), fruit(662, 241);
/**
 * A static helper function that returns the time with millisecond precision.
 */
long getTime();


void on_mouse(int event, int x, int y, int flags, void *)
{
	static int count = 0;

	switch(event) {
		case CV_EVENT_LBUTTONDOWN:
			if (count == 0) {
				robot = Point(x, y);
				cout << "Got robot position: (" << x << ", " << y << ")" << endl;
			} else if (count == 1) {
				fruit = Point(x, y);
				cout << "Got fruit position: (" << x << ", " << y << ")" << endl;
			}

			count++;
			break;
	}
}

int manhatten(Point a, Point b) {
	return abs(a.x - b.x) + abs(a.y - b.y);
}

void showPlan(const Mat &image, const vector<Point> plan, int blockSize, int next = -1) {
	Mat temp = image.clone();
	int count = 0;

	static bool first = true;
	int total = 0;

	for (vector<Point>::const_iterator it = plan.begin(); it != plan.end(); ++it) {
		Point p(it->x * blockSize + blockSize / 2,
			it->y * blockSize + blockSize / 2);

		if (count == next) {
			circle(temp, p, 7, CV_RGB(0, 0, 255), CV_FILLED);
		} else {
			circle(temp, p, 5, CV_RGB(255, 0, 0), CV_FILLED);
		}

		count++;
		vector<Point>::const_iterator next = it + 1;

		if (next != plan.end()) {
			Point a(it->x * blockSize + blockSize / 2,
				it->y * blockSize + blockSize / 2);
			Point b(next->x * blockSize + blockSize / 2,
				next->y * blockSize + blockSize / 2);

			line(temp, a, b, CV_RGB(255, 0, 0));
			total += manhatten(a, b);
		}
	}

	if (first) {
		cout << "Total path length: " << total << endl;
		first = false;
	}

	imshow("Plan", temp);
	cvMoveWindow("Plan", 0, 900 - temp.size().height);
}

int main(int, char **) {
    long startTime;
	cout << "Press 'n' to step forward to the next frame." << endl
		<< "Press 'q' to close." << endl
		<< "In debug view:" << endl
		<< "\tRed is obstacle" << endl
		<< "\tGreen is fruit" << endl
		<< "\tBlue is robot" << endl;

	LiveSource *source = new LiveSource(0);
	ObjectDetector detector(source, false);

	//source->setImageFilename(filename("background"));
	detector.saveBackground();


	imshow("Processed", detector.processed());
	imshow("Raw", source->next());

	cvMoveWindow("Processed", 0, 0);
	cvMoveWindow("Raw", source->next().size().width, 0);

	while (cvWaitKey(300) != 'n'){
	    imshow("Raw", source->next());


	    if(cvWaitKey(300) == 'q')
            return 0;
	};

    //source->setImageFilename(filename("obstacles"));
	detector.saveObstacles();


	imshow("Processed", detector.processed());
	imshow("Raw", source->next());

	while (cvWaitKey(300) != 'n'){
        imshow("Raw", source->next());
	    if(cvWaitKey(300) == 'q')
            return 0;
	};


	//source->setImageFilename(filename("robot_and_fruit", 0));
	imshow("Live", source->next());
	cvSetMouseCallback("Live", on_mouse, 0);

	cout << "Click near the robot and fruit (in that order), then press 'c'"
		<< endl;

	int key;

	while (true) {
		key = waitKey(300);

		if (key == 'q') {
			return 0;
		} else if (key == 'c') {
			break;
		}
	}

	cvDestroyWindow("Live");

	if (DEBUG) {
		cvNamedWindow("Robot Blobs");
		cvMoveWindow("Robot Blobs", source->next().size().width,
			900 - source->next().size().height);
	}

	detector.saveRobotAndFruit(robot, fruit, DEBUG);

    Mat open = detector.getOpen();
    ApproxDecomp decomp(source, open);

    Mat grid = decomp.cellDecomp();
	CellDecompAStar astar(grid, new ManhattenDistanceHeuristic());

	imshow("Processed", detector.processed());
	imshow("Raw", source->next());

	Point robot = detector.robot().point;
	Point fruit = detector.fruit();

	Point start(robot.x / decomp.blockSize(), robot.y / decomp.blockSize());
	Point goal(fruit.x / decomp.blockSize(), fruit.y / decomp.blockSize());

	vector<Point> plan = astar.plan(start, goal);
	//cout << "target point = " << plan[0].x << " " << plan[0].y;
    std::cout << "Planning time: " << getTime() - startTime << std::endl;
	showPlan(source->next(), plan, decomp.blockSize());
    startTime = getTime();
	DUMP(plan);

	SimpleDriveController *controller = new SimpleDriveController();
	Rovio rovio(ROVIO_HOST);
	size_t step = 0; // The step in the plan

	for (int i = 0; i < 2; i++) {
        startTime = getTime();
		while (step < plan.size()) {

			/*int key = cvWaitKey(300);

			if (key == 'q') {
				break;
			}*/

			RobotOrientation current = detector.findRobot(DEBUG);
			Command command;

			do {
                cv::Point goal = Point(plan[step].x*decomp.blockSize(),plan[step].y*decomp.blockSize());

                command = controller->nextOmni(current, goal);
			    string str;


                 switch(command.direction){
                     case 0:
                     str = "DIR_STOP";
                     break;
                     case 1:
                     str = "DIR_FORWARD";
                     break;
                     case 2:
                     str = "DIR_BACKWARD";
                     break;
                     case 3:
                     str = "DIR_LEFT";
                     break;
                     case 4:
                     str = "DIR_RIGHT";
                     break;
                     case 5:
                     str = "DIR_ROT_LEFT";
                     break;
                     case 6:
                     str = "DIR_ROT_RIGHT";
                     break;
                     case 7:
                     str = "DIR_DIAG_FOR_LEFT";
                     break;
                     case 8:
                     str = "DIR_DIAG_FOR_RIGHT";
                     break;
                     case 9:
                     str = "DIR_DIAG_BACK_LEFT";
                     break;
                     case 10:
                     str = "DIR_DIAG_BACK_RIGHT";
                     break;
                     case 11:
                     str = "HEAD_UP";
                     break;
                     case 12:
                     str = "HEAD_DOWN";
                     break;
                     case 13:
                     str = "HEAD_MIDDLE";
                     break;
                     default:
                     str = "invalid direction";
                 }


            cout  << "Robot position: " << current.point.x << "," << current.point.y << endl;
            cout << "Goal position: " << goal.x << "," << goal.y << endl;
            cout << "Robot angle: " << 360- current.angle << endl;
            cout << "COMMAND: " << str << endl;





				if (command.direction == DIR_STOP) {
					step++;
					cout << "Moving on to next step" << endl;
				}
			} while (command.direction == DIR_STOP);



			showPlan(source->next(), plan, decomp.blockSize(), step);

			imshow("Processed", detector.processed());
			imshow("Raw", source->next());

			waitKey(1000);
			rovio.timeDrive(command.direction, command.time, command.speed);
			//cout << endl << "For Robot (" << current.point.x <<","<< current.point.y<<") with angle " << current.angle << en
			//cout << "Command: " << command.direction <<endl;
		}

		step = 0;
        std::cout << "Time To the Fruit: " << getTime() - startTime << std::endl;

		//reverse(plan.begin(), plan.end());
	}
	return 0;
}


long getTime() {
    struct timeval time;
    gettimeofday(&time, NULL);
    return ((time.tv_sec) * 1000 + time.tv_usec/1000.0) + 0.5;
}

